package com.ld.control.common.equipment.method;


import com.alibaba.fastjson.JSONArray;
import com.alibaba.fastjson.JSONObject;
import com.ld.control.business.process.model.ProcessNodeExampleRunningParamModel;
import com.ld.control.business.process.model.ProcessNodeParamExampleModel;
import com.ld.control.business.robot.service.HuiLingRobotPointService;
import com.ld.control.business.robot.service.HuiLingRobotPointVariableService;
import com.ld.control.business.robot.vo.internal.HuiLingRobotPointVariableExtVo;
import com.ld.control.business.robot.vo.resp.HuiLingRobotPointRespVo;
import com.ld.control.business.robot.vo.resp.HuiLingRobotPointVariableRespVo;
import com.ld.control.business.variable.service.VariableExampleService;
import com.ld.control.business.variable.vo.resp.VariableExampleValueResp;
import com.ld.control.common.equipment.param.HuiLingRobotMethodParamConstant;
import com.ld.control.common.vo.EquipmentSystemMethodVo;
import com.ld.control.sysenum.MethodParamTypeEnum;
import lombok.extern.slf4j.Slf4j;
import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.stereotype.Component;
import org.springframework.util.CollectionUtils;

import java.util.ArrayList;
import java.util.List;
import java.util.Map;

@Slf4j
@Component
public class HuiLinEquipmentSystemMethod {

    @Autowired
    private HuiLingRobotPointVariableService huiLingRobotPointVariableService;

    @Autowired
    private HuiLingRobotPointService huiLingRobotPointService;

    @Autowired
    private VariableExampleService variableExampleService;

    public EquipmentSystemMethodVo getHui_Ling_Pick_Ext(Long processNodeExampleRunningModelId, Map<String, ProcessNodeParamExampleModel> processNodeParamExampleModeMap) {
        EquipmentSystemMethodVo equipmentSystemMethodVo = new EquipmentSystemMethodVo();
        List<ProcessNodeExampleRunningParamModel> processNodeExampleRunningParamModelList = new ArrayList<>();
        ProcessNodeParamExampleModel robotPointNameVo = processNodeParamExampleModeMap.get("robotPointName");
        ProcessNodeParamExampleModel jawDistance = processNodeParamExampleModeMap.get("jawDistance");
        ProcessNodeParamExampleModel jawForceLimit = processNodeParamExampleModeMap.get("jawForceLimit");
        ProcessNodeParamExampleModel initDistance = processNodeParamExampleModeMap.get("initDistance");

        // 获取点位信息
        String robotPointName = robotPointNameVo.getValue();
        Long processNodeExampleId = robotPointNameVo.getProcessNodeExampleId();
        HuiLingRobotPointVariableExtVo huiLingRobotPointVariableExtVo = getHuiLingRobotPointVo(robotPointNameVo);
        assert huiLingRobotPointVariableExtVo != null : "huiLingRobotPointVariableExtVo is null";
        List<HuiLingRobotPointVariableRespVo> targetVariableList = huiLingRobotPointVariableExtVo.getTargetVariableList();
        List<HuiLingRobotPointVariableRespVo> initVariableList = huiLingRobotPointVariableExtVo.getInitVariableList();

        String targetPointXYZValueJsonStr="";
        String targetSafePointXYZValueSJsonStr="";
        if (!CollectionUtils.isEmpty(targetVariableList) && !CollectionUtils.isEmpty(initVariableList)) {
            targetPointXYZValueJsonStr = generateXYZR(targetVariableList);
            targetSafePointXYZValueSJsonStr = generateXYZR(initVariableList);
        }

        equipmentSystemMethodVo.setEquipmentMethodName(HuiLingRobotMethodParamConstant.Pick.methodName);

        ProcessNodeExampleRunningParamModel targetPoint_XYZParam = new ProcessNodeExampleRunningParamModel();
        targetPoint_XYZParam.setProcessNodeExampleRunningId(processNodeExampleRunningModelId);
        targetPoint_XYZParam.setSendMethodParamName(HuiLingRobotMethodParamConstant.Pick.targetPoint_XYZ);
        targetPoint_XYZParam.setSendMethodParamValue(targetPointXYZValueJsonStr);
        targetPoint_XYZParam.setParamType(MethodParamTypeEnum.ARRAY.getType());

        ProcessNodeExampleRunningParamModel targetSafePoint_XYZParam = new ProcessNodeExampleRunningParamModel();
        targetSafePoint_XYZParam.setProcessNodeExampleRunningId(processNodeExampleRunningModelId);
        targetSafePoint_XYZParam.setSendMethodParamName(HuiLingRobotMethodParamConstant.Pick.targetSafePoint_XYZ);
        targetSafePoint_XYZParam.setSendMethodParamValue(targetSafePointXYZValueSJsonStr);
        targetSafePoint_XYZParam.setParamType(MethodParamTypeEnum.ARRAY.getType());

        ProcessNodeExampleRunningParamModel jawDistanceParam = new ProcessNodeExampleRunningParamModel();
        jawDistanceParam.setProcessNodeExampleRunningId(processNodeExampleRunningModelId);
        jawDistanceParam.setSendMethodParamName(HuiLingRobotMethodParamConstant.Pick.jawDistance);
        jawDistanceParam.setSendMethodParamValue(String.valueOf(jawDistance.getValue()));
        jawDistanceParam.setParamType(MethodParamTypeEnum.STRING.getType());

        ProcessNodeExampleRunningParamModel jawForceLimitParam = new ProcessNodeExampleRunningParamModel();
        jawForceLimitParam.setProcessNodeExampleRunningId(processNodeExampleRunningModelId);
        jawForceLimitParam.setSendMethodParamName(HuiLingRobotMethodParamConstant.Pick.jawForceLimit);
        jawForceLimitParam.setSendMethodParamValue(String.valueOf(jawForceLimit.getValue()));
        jawForceLimitParam.setParamType(MethodParamTypeEnum.STRING.getType());

        ProcessNodeExampleRunningParamModel initDistanceParam = new ProcessNodeExampleRunningParamModel();
        initDistanceParam.setProcessNodeExampleRunningId(processNodeExampleRunningModelId);
        initDistanceParam.setSendMethodParamName(HuiLingRobotMethodParamConstant.Pick.initDistance);
        initDistanceParam.setSendMethodParamValue(String.valueOf(initDistance.getValue()));
        initDistanceParam.setParamType(MethodParamTypeEnum.STRING.getType());

        //数据加入到集合
        processNodeExampleRunningParamModelList.add(targetPoint_XYZParam);
        processNodeExampleRunningParamModelList.add(targetSafePoint_XYZParam);
        processNodeExampleRunningParamModelList.add(jawDistanceParam);
        processNodeExampleRunningParamModelList.add(jawForceLimitParam);
        processNodeExampleRunningParamModelList.add(initDistanceParam);
        equipmentSystemMethodVo.setProcessNodeExampleRunningParamModelList(processNodeExampleRunningParamModelList);
        return equipmentSystemMethodVo;
    }

    public EquipmentSystemMethodVo getHui_Ling_Place_Ext(Long processNodeExampleRunningModelId, Map<String, ProcessNodeParamExampleModel> processNodeParamExampleModeMap) {
        EquipmentSystemMethodVo equipmentSystemMethodVo = new EquipmentSystemMethodVo();
        List<ProcessNodeExampleRunningParamModel> processNodeExampleRunningParamModelList = new ArrayList<>();
        ProcessNodeParamExampleModel robotPointNameVo = processNodeParamExampleModeMap.get("robotPointName");
        ProcessNodeParamExampleModel initDistance = processNodeParamExampleModeMap.get("initDistance");

        // 获取点位信息
        HuiLingRobotPointVariableExtVo huiLingRobotPointVariableExtVo = getHuiLingRobotPointVo(robotPointNameVo);
        assert huiLingRobotPointVariableExtVo != null : "huiLingRobotPointVariableExtVo is null";
        List<HuiLingRobotPointVariableRespVo> targetVariableList = huiLingRobotPointVariableExtVo.getTargetVariableList();
        List<HuiLingRobotPointVariableRespVo> initVariableList = huiLingRobotPointVariableExtVo.getInitVariableList();

        String targetPointXYZValueJsonStr="";
        String targetSafePointXYZValueSJsonStr="";
        if (!CollectionUtils.isEmpty(targetVariableList) && !CollectionUtils.isEmpty(initVariableList)) {
            targetPointXYZValueJsonStr = generateXYZR(targetVariableList);
            targetSafePointXYZValueSJsonStr = generateXYZR(initVariableList);
        }

        equipmentSystemMethodVo.setEquipmentMethodName(HuiLingRobotMethodParamConstant.Place.methodName);

        ProcessNodeExampleRunningParamModel targetPoint_XYZParam = new ProcessNodeExampleRunningParamModel();
        targetPoint_XYZParam.setProcessNodeExampleRunningId(processNodeExampleRunningModelId);
        targetPoint_XYZParam.setSendMethodParamName(HuiLingRobotMethodParamConstant.Place.targetPoint_XYZ);
        targetPoint_XYZParam.setSendMethodParamValue(targetPointXYZValueJsonStr);
        targetPoint_XYZParam.setParamType(MethodParamTypeEnum.ARRAY.getType());

        ProcessNodeExampleRunningParamModel targetSafePoint_XYZParam = new ProcessNodeExampleRunningParamModel();
        targetSafePoint_XYZParam.setProcessNodeExampleRunningId(processNodeExampleRunningModelId);
        targetSafePoint_XYZParam.setSendMethodParamName(HuiLingRobotMethodParamConstant.Place.targetSafePoint_XYZ);
        targetSafePoint_XYZParam.setSendMethodParamValue(targetSafePointXYZValueSJsonStr);
        targetSafePoint_XYZParam.setParamType(MethodParamTypeEnum.ARRAY.getType());


        ProcessNodeExampleRunningParamModel initDistanceParam = new ProcessNodeExampleRunningParamModel();
        initDistanceParam.setProcessNodeExampleRunningId(processNodeExampleRunningModelId);
        initDistanceParam.setSendMethodParamName(HuiLingRobotMethodParamConstant.Place.initDistance);
        initDistanceParam.setSendMethodParamValue(String.valueOf(initDistance.getValue()));
        initDistanceParam.setParamType(MethodParamTypeEnum.STRING.getType());

        //数据加入到集合
        processNodeExampleRunningParamModelList.add(targetPoint_XYZParam);
        processNodeExampleRunningParamModelList.add(targetSafePoint_XYZParam);
        processNodeExampleRunningParamModelList.add(initDistanceParam);
        equipmentSystemMethodVo.setProcessNodeExampleRunningParamModelList(processNodeExampleRunningParamModelList);
        return equipmentSystemMethodVo;
    }

    public EquipmentSystemMethodVo getHui_Ling_SingleAction_Ext(Long processNodeExampleRunningModelId, Map<String, ProcessNodeParamExampleModel> processNodeParamExampleModeMap) {
        EquipmentSystemMethodVo equipmentSystemMethodVo = new EquipmentSystemMethodVo();
        List<ProcessNodeExampleRunningParamModel> processNodeExampleRunningParamModelList = new ArrayList<>();
        ProcessNodeParamExampleModel robotPointNameVo = processNodeParamExampleModeMap.get("robotPointName");
        ProcessNodeParamExampleModel initDistance = processNodeParamExampleModeMap.get("initDistance");
        // 获取点位信息
        HuiLingRobotPointVariableExtVo huiLingRobotPointVariableExtVo = getHuiLingRobotPointVo(robotPointNameVo);
        assert huiLingRobotPointVariableExtVo != null : "huiLingRobotPointVariableExtVo is null";
        List<HuiLingRobotPointVariableRespVo> targetVariableList = huiLingRobotPointVariableExtVo.getTargetVariableList();
        String targetPointXYZValueJsonStr="";
        if (!CollectionUtils.isEmpty(targetVariableList)) {
            targetPointXYZValueJsonStr = generateXYZR(targetVariableList);
        }

        ProcessNodeExampleRunningParamModel targetPoint_XYZParam = new ProcessNodeExampleRunningParamModel();
        targetPoint_XYZParam.setProcessNodeExampleRunningId(processNodeExampleRunningModelId);
        targetPoint_XYZParam.setSendMethodParamName(HuiLingRobotMethodParamConstant.SingleAction.targetPoint_XYZ);
        targetPoint_XYZParam.setSendMethodParamValue(targetPointXYZValueJsonStr);
        targetPoint_XYZParam.setParamType(MethodParamTypeEnum.ARRAY.getType());

        ProcessNodeExampleRunningParamModel initDistanceParam = new ProcessNodeExampleRunningParamModel();
        initDistanceParam.setProcessNodeExampleRunningId(processNodeExampleRunningModelId);
        initDistanceParam.setSendMethodParamName(HuiLingRobotMethodParamConstant.SingleAction.initDistance);
        initDistanceParam.setSendMethodParamValue(String.valueOf(initDistance.getValue()));
        initDistanceParam.setParamType(MethodParamTypeEnum.STRING.getType());

        //数据加入到集合
        processNodeExampleRunningParamModelList.add(targetPoint_XYZParam);
        processNodeExampleRunningParamModelList.add(initDistanceParam);
        equipmentSystemMethodVo.setEquipmentMethodName(HuiLingRobotMethodParamConstant.SingleAction.methodName);
        equipmentSystemMethodVo.setProcessNodeExampleRunningParamModelList(processNodeExampleRunningParamModelList);
        return equipmentSystemMethodVo;
    }


    public EquipmentSystemMethodVo getHui_Ling_DoubleAction_Ext(Long processNodeExampleRunningModelId, Map<String, ProcessNodeParamExampleModel> processNodeParamExampleModeMap) {
        EquipmentSystemMethodVo equipmentSystemMethodVo = new EquipmentSystemMethodVo();
        List<ProcessNodeExampleRunningParamModel> processNodeExampleRunningParamModelList = new ArrayList<>();
        ProcessNodeParamExampleModel robotPointNameVo = processNodeParamExampleModeMap.get("robotPointName");
        ProcessNodeParamExampleModel initDistance = processNodeParamExampleModeMap.get("initDistance");

        // 获取点位信息
        HuiLingRobotPointVariableExtVo huiLingRobotPointVariableExtVo = getHuiLingRobotPointVo(robotPointNameVo);
        assert huiLingRobotPointVariableExtVo != null : "huiLingRobotPointVariableExtVo is null";
        List<HuiLingRobotPointVariableRespVo> targetVariableList = huiLingRobotPointVariableExtVo.getTargetVariableList();
        List<HuiLingRobotPointVariableRespVo> initVariableList = huiLingRobotPointVariableExtVo.getInitVariableList();

        String targetPointXYZValueJsonStr="";
        String targetSafePointXYZValueSJsonStr="";
        if (!CollectionUtils.isEmpty(targetVariableList) && !CollectionUtils.isEmpty(initVariableList)) {
            targetPointXYZValueJsonStr = generateXYZR(targetVariableList);
            targetSafePointXYZValueSJsonStr = generateXYZR(initVariableList);
        }

        ProcessNodeExampleRunningParamModel targetPoint_XYZParam = new ProcessNodeExampleRunningParamModel();
        targetPoint_XYZParam.setProcessNodeExampleRunningId(processNodeExampleRunningModelId);
        targetPoint_XYZParam.setSendMethodParamName(HuiLingRobotMethodParamConstant.DoubleAction.targetPoint_XYZ);
        targetPoint_XYZParam.setSendMethodParamValue(targetPointXYZValueJsonStr);
        targetPoint_XYZParam.setParamType(MethodParamTypeEnum.ARRAY.getType());

        ProcessNodeExampleRunningParamModel targetSafePoint_XYZParam = new ProcessNodeExampleRunningParamModel();
        targetSafePoint_XYZParam.setProcessNodeExampleRunningId(processNodeExampleRunningModelId);
        targetSafePoint_XYZParam.setSendMethodParamName(HuiLingRobotMethodParamConstant.DoubleAction.targetSafePoint_XYZ);
        targetSafePoint_XYZParam.setSendMethodParamValue(targetSafePointXYZValueSJsonStr);
        targetSafePoint_XYZParam.setParamType(MethodParamTypeEnum.ARRAY.getType());


        ProcessNodeExampleRunningParamModel initDistanceParam = new ProcessNodeExampleRunningParamModel();
        initDistanceParam.setProcessNodeExampleRunningId(processNodeExampleRunningModelId);
        initDistanceParam.setSendMethodParamName(HuiLingRobotMethodParamConstant.DoubleAction.initDistance);
        initDistanceParam.setSendMethodParamValue(String.valueOf(initDistance.getValue()));
        initDistanceParam.setParamType(MethodParamTypeEnum.STRING.getType());

        //数据加入到集合
        processNodeExampleRunningParamModelList.add(targetPoint_XYZParam);
        processNodeExampleRunningParamModelList.add(targetSafePoint_XYZParam);
        processNodeExampleRunningParamModelList.add(initDistanceParam);
        equipmentSystemMethodVo.setProcessNodeExampleRunningParamModelList(processNodeExampleRunningParamModelList);
        equipmentSystemMethodVo.setEquipmentMethodName(HuiLingRobotMethodParamConstant.DoubleAction.methodName);
        return equipmentSystemMethodVo;
    }

    private HuiLingRobotPointVariableExtVo getHuiLingRobotPointVo(ProcessNodeParamExampleModel processNodeParamExampleModel) {



        HuiLingRobotPointVariableExtVo huiLingRobotPointVariableExtVo = new HuiLingRobotPointVariableExtVo();
        List<HuiLingRobotPointVariableRespVo> targetVariableList = new ArrayList<>();
        List<HuiLingRobotPointVariableRespVo> initVariableList = new ArrayList<>();

        String huiLingRobotPointName = processNodeParamExampleModel.getValue();
        Long processNodeExampleId = processNodeParamExampleModel.getProcessNodeExampleId();






        log.info("getHuiLingRobotPointVo:{}", huiLingRobotPointName);
        if (huiLingRobotPointName.startsWith("@")) {
            VariableExampleValueResp byNameExt = variableExampleService.getByNameExt(processNodeExampleId, huiLingRobotPointName);
            huiLingRobotPointName = byNameExt.getCurrentValue();
        }
        HuiLingRobotPointRespVo huiLingRobotPointModel = huiLingRobotPointService.getByName(huiLingRobotPointName);
        if (huiLingRobotPointModel == null) {
            log.info("{} 点位不存在", huiLingRobotPointName);
            return null;
        }

        List<String> targetPointVariableNameList = huiLingRobotPointModel.getTargetPointVariableNameList();
        List<String> initPointVariableNameList = huiLingRobotPointModel.getInitPointVariableNameList();

        HuiLingRobotPointVariableRespVo huiLingRobotPointVariableRespVo;

        if (!CollectionUtils.isEmpty(targetPointVariableNameList)) {
            for (String targetPointVariableName : targetPointVariableNameList) {
                huiLingRobotPointVariableRespVo = huiLingRobotPointVariableService.getByName(targetPointVariableName);
                targetVariableList.add(huiLingRobotPointVariableRespVo);
            }
        }
        if (!CollectionUtils.isEmpty(initPointVariableNameList)) {
            for (String initPointVariableName : initPointVariableNameList) {
                huiLingRobotPointVariableRespVo = huiLingRobotPointVariableService.getByName(initPointVariableName);
                initVariableList.add(huiLingRobotPointVariableRespVo);
            }
        }
        huiLingRobotPointVariableExtVo.setTargetVariableList(targetVariableList);
        huiLingRobotPointVariableExtVo.setInitVariableList(initVariableList);
        return huiLingRobotPointVariableExtVo;
    }

    /**
     * "x": 0,
     * "y": 0,
     *  "z": 0,
     *  "r": 0,
     *  "speed": 0,
     *  "lr": 1,//-1:left 1:right
     *  "motiontype": 1//0:MoveJ 1:MoveL
     * @param huiLingRobotPointVariableRespVoList
     * @return
     */
    private String generateXYZR(List<HuiLingRobotPointVariableRespVo> huiLingRobotPointVariableRespVoList){
        JSONArray jsonArray = new JSONArray();
        for (HuiLingRobotPointVariableRespVo huiLingRobotPointVo : huiLingRobotPointVariableRespVoList) {
            JSONObject jsonObject = new JSONObject();
            jsonObject.put("x", huiLingRobotPointVo.getAxisX());
            jsonObject.put("y", huiLingRobotPointVo.getAxisY());
            jsonObject.put("z", huiLingRobotPointVo.getAxisZ());
            jsonObject.put("r", huiLingRobotPointVo.getAxisR());
            //jsonObject.put("speed", );
            jsonObject.put("lr", huiLingRobotPointVo.getArmDirection());
            jsonObject.put("motiontype", huiLingRobotPointVo.getMotionType());
            jsonArray.add(jsonObject);
        }
        return jsonArray.toJSONString();

    }

}
